/*
 *        (C) COPYRIGHT Ingenic Limited.
 *             ALL RIGHTS RESERVED
 *
 * File       : navigationnode.cpp
 * Authors    : ycai@ubuntu
 * Create Time: 2022-06-09:19:18:52
 * Description:
 * 
 */
#include "ros/ros.h"
#include "ros/console.h"
#include "sensor_msgs/PointCloud.h"
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/Imu.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf2/LinearMath/Transform.h"
#include "tf2/convert.h"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "message_filters/subscriber.h"
#include <std_msgs/String.h>
#include <std_msgs/Header.h>
#include "nav_msgs/GetMap.h"
#include <mutex>
#include <shared_mutex>
#include <sys/time.h>

#include "csm.h"
#include "lib_point_cloud_raw.h"
#include "lib_carto_contour.h"
#include "lib_composite_ICP.h"



#define NumcalculationInitPose 1

using namespace std;

extern point_cloud_type point_cloud_prev(NUM_POINTS_STANDARD_SCAN);
extern point_cloud_type point_cloud_new(NUM_POINTS_STANDARD_SCAN);
extern point_cloud_type point_cloud_map(NUM_POINTS_STANDARD_SCAN);
extern point_cloud_type point_cloud_ref(NUM_POINTS_STANDARD_SCAN);


//TODO:accomplish laser(wheel)-odom and IMU-odom in this Node,stored in map container and interpl the pose!

class navigationnode 
{
  public:
  navigationnode();
  
  ~navigationnode();

  void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg);

  void function_pub(pose_type pose,double P_element[6],const sensor_msgs::LaserScan::ConstPtr& msg,ros::Publisher pose_pub);

  void imuCallback(const sensor_msgs::ImuConstPtr &imu);

  void wheelCallback(const geometry_msgs::PoseStamped::ConstPtr& pose);

  bool getOdomPose(double& x, double& y, double& yaw,double time);

  void laserScanToLDP(const point_cloud_type* std_point, LDP& laser, double range_max);
    
  carto_map_type* carto_map;
    
  ros::NodeHandle nh_;


  double pose_x[NumcalculationInitPose] = { 0.0 };
  double pose_y[NumcalculationInitPose] = { 0.0 };
  double pose_th[NumcalculationInitPose]= { 0.0 };
  
  
  private:
  std::map<double, pose_type> imu_odom_pose;//存储里程计位姿数据，队列中存储10s的数据大小
  
  std::map<double, pose_type> wheel_odom_pose;

  std::shared_timed_mutex mtx_imu_odom_pose;

  std::shared_timed_mutex mtx_wheel_odom_pose;

  pose_type imu_pose = { 0.0, 0.0, 0.0 };

  double imu_angle_0 = 0.0;

  double velocity[2] = { 0.0, 0.0 };
  
  bool first_in_imu = true;

  bool first_in_laser = true;

  ros::Time last_imu_time;
  
  double dt;

  int imu_init_num = 50;
  
  pose_type laser_scanner_pose;

  double P_element[6];

  ros::Subscriber laser_scan_sub_;

  ros::Subscriber imu_data_sub_;

  ros::Subscriber wheel_odom_sub_;
    
  ros::Publisher laserpose_pub_;
      
  boost::mutex laserpose_mutex;
  
  bool init_pose_flag = true;

  ros::Time last_scan_time;
  
  string pgm_file_path,yaml_file_path,occupy_file_path,curt_position_file_path;
  
  int pose_abnormal_count = 1;

  pose_type odo = { 0.0, 0.0, 0.0 };
  pose_type odo_prev = { 0.0, 0.0, 0.0 };
  pose_type delta_pose = { 0.0, 0.0, 0.0 };
  pose_type delta_pose_prev = { 0.0, 0.0, 0.0 };
  pose_type ref_map_pose = { 0.0, 0.0, 0.0 };
  double refmap_sigma_array[6] = { 0.0 };
  int cal_num = 0;

  CSM* csm_s2m;
  CSM* csm_s2s;
  CSM* csm_s2k;
  CSM* csm_k2m;

  struct sm_params params_s2m;  
  struct sm_result result_s2m;

  struct sm_params params_s2s;  
  struct sm_result result_s2s;

  struct sm_params params_s2k;  
  struct sm_result result_s2k;

  struct sm_params params_k2m;  
  struct sm_result result_k2m;
};


//construction function
navigationnode::navigationnode()
{
      
  nh_.getParam("/relocalizationCensiICP/pgm_file_path", pgm_file_path);//Get the map data and curt_position file path.
  nh_.getParam("/relocalizationCensiICP/yaml_file_path", yaml_file_path);
  nh_.getParam("/relocalizationCensiICP/occupy_file_path", occupy_file_path);
  nh_.getParam("/relocalizationCensiICP/curt_position_file_path", curt_position_file_path);
      
  laser_scanner_pose = { 0.0, 0.0, 0.0 };
  P_element[6] = { 0.0 };

  carto_map = new carto_map_type(yaml_file_path.c_str(), pgm_file_path.c_str());

  csm_s2m = new CSM();
  csm_s2s = new CSM();
  csm_s2k = new CSM();
  csm_k2m = new CSM();

  laser_scan_sub_ = nh_.subscribe("/scan", 1 , &navigationnode::laserCallback , this); 
  laserpose_pub_ = nh_.advertise<const geometry_msgs::PoseWithCovarianceStamped>("laserpose", 2 ,true);
  imu_data_sub_ = nh_.subscribe("/imu/data", 1 , &navigationnode::imuCallback , this);
  wheel_odom_sub_ = nh_.subscribe("/wheel/data", 1 , &navigationnode::wheelCallback , this);

  params_s2m.sigma = 0.01;
  params_s2m.use_corr_tricks = 1;
  params_s2m.max_correspondence_dist = 1.5;
  params_s2m.max_iterations = 10;
  params_s2m.outliers_maxPerc = 0.9;
  params_s2m.outliers_adaptive_mult = 2.0;
  params_s2m.outliers_adaptive_order = 0.85;
  params_s2m.outliers_remove_doubles = 1;
  params_s2m.use_point_to_line_distance = 1;
  params_s2m.do_alpha_test = 0;
  params_s2m.do_compute_covariance = 1;
  params_s2m.use_ml_weights = 0;
  params_s2m.use_sigma_weights = 0;
  params_s2m.min_reading = 0.0;
  params_s2m.max_reading = 30.0;
  params_s2m.laser[0] = 0.0;
  params_s2m.laser[1] = 0.0;
  params_s2m.laser[2] = 0.0;

  params_s2s.sigma = 0.01;
  params_s2s.use_corr_tricks = 1;
  params_s2s.max_correspondence_dist = 1.5;
  params_s2s.max_iterations = 10;
  params_s2s.outliers_maxPerc = 0.9;
  params_s2s.outliers_adaptive_mult = 2.0;
  params_s2s.outliers_adaptive_order = 0.85;
  params_s2s.outliers_remove_doubles = 1;
  params_s2s.use_point_to_line_distance = 1;
  params_s2s.do_alpha_test = 0;
  params_s2s.do_compute_covariance = 0;
  params_s2s.use_ml_weights = 0;
  params_s2s.use_sigma_weights = 0;
  params_s2s.min_reading = 0.0;
  params_s2s.max_reading = 30.0;
  params_s2s.laser[0] = 0.0;
  params_s2s.laser[1] = 0.0;
  params_s2s.laser[2] = 0.0;

  params_s2k.sigma = 0.01;
  params_s2k.use_corr_tricks = 1;
  params_s2k.max_correspondence_dist = 1.5;
  params_s2k.max_iterations = 10;
  params_s2k.outliers_maxPerc = 0.9;
  params_s2k.outliers_adaptive_mult = 2.0;
  params_s2k.outliers_adaptive_order = 0.85;
  params_s2k.outliers_remove_doubles = 1;
  params_s2k.use_point_to_line_distance = 1;
  params_s2k.do_alpha_test = 0;
  params_s2k.do_compute_covariance = 1;
  params_s2k.use_ml_weights = 0;
  params_s2k.use_sigma_weights = 0;
  params_s2k.min_reading = 0.0;
  params_s2k.max_reading = 30.0;
  params_s2k.laser[0] = 0.0;
  params_s2k.laser[1] = 0.0;
  params_s2k.laser[2] = 0.0;

  params_k2m.sigma = 0.01;
  params_k2m.use_corr_tricks = 1;
  params_k2m.max_correspondence_dist = 1.5;
  params_k2m.max_iterations = 10;
  params_k2m.outliers_maxPerc = 0.9;
  params_k2m.outliers_adaptive_mult = 2.0;
  params_k2m.outliers_adaptive_order = 0.85;
  params_k2m.outliers_remove_doubles = 1;
  params_k2m.use_point_to_line_distance = 1;
  params_k2m.do_alpha_test = 0;
  params_k2m.do_compute_covariance = 1;
  params_k2m.use_ml_weights = 0;
  params_k2m.use_sigma_weights = 0;
  params_k2m.min_reading = 0.0;
  params_k2m.max_reading = 30.0;
  params_k2m.laser[0] = 0.0;
  params_k2m.laser[1] = 0.0;
  params_k2m.laser[2] = 0.0;
  
  double corres_ratio = 0.0;
}


//deconstruction function
navigationnode::~navigationnode()
{

  delete carto_map;
  delete csm_s2m;
  delete csm_s2s;
  delete csm_s2k;
  delete csm_k2m;
}


void navigationnode::imuCallback(const sensor_msgs::ImuConstPtr &imu)
{
  if(first_in_imu)
    {
      static int imu_cal_num = 0;
      if(imu_cal_num < imu_init_num)
	{
	  double t_yaw;  
	  t_yaw = tf2::getYaw(imu -> orientation);
	  t_yaw = atan2(sin(t_yaw), cos(t_yaw));
	  if(t_yaw < -3.12)//178deg
	    t_yaw += 2*PI;
	  imu_pose.theta += t_yaw;
	  imu_cal_num++;
	}
      if(imu_cal_num == imu_init_num)
	{
	  //calculation imu's start yaw
	  imu_angle_0 = (double)imu_pose.theta / imu_init_num;
	  imu_angle_0 = atan2(sin(imu_angle_0), cos(imu_angle_0));
	  first_in_imu = false;
	  imu_pose.x = 0;
	  imu_pose.y = 0;
	  imu_pose.theta = 0.0;	  
	  imu_odom_pose.insert(pair<double, pose_type>(imu -> header.stamp.toSec(),imu_pose));

	  //setGravity(imu->linear_acceleration);
	  last_imu_time = imu -> header.stamp;
	}
    }
  else
    {
      std::unique_lock<std::shared_timed_mutex> pose_lock(mtx_imu_odom_pose);
      
      dt = (imu -> header.stamp - last_imu_time).toSec();

      last_imu_time = imu -> header.stamp;
            
      pose_type d_pose = {0.0,0.0,0.0};
      if(fabs(imu->linear_acceleration.x) >= 0.1)
	{
	  velocity[0] = velocity[0] + imu->linear_acceleration.x * dt;
	  d_pose.x = velocity[0] * dt;
	  //d_pose.x = imu->linear_acceleration.x * dt * dt;
	}
      else
	{
	  //d_pose.x = velocity[0] * dt;
	}
      if(fabs(imu->linear_acceleration.y) >= 0.1)
	{
	  velocity[1] = velocity[1] + imu->linear_acceleration.y * dt;
	  d_pose.y = velocity[1] * dt;
	  //d_pose.y = imu->linear_acceleration.y * dt * dt;
	}
      else
	{
	  //d_pose.y = velocity[1] * dt;
	}      
      imu_pose = pose_transform(&d_pose, &imu_pose);
      imu_pose.theta = tf2::getYaw(imu->orientation) - imu_angle_0;
      imu_pose.theta = atan2(sin(imu_pose.theta), cos(imu_pose.theta));
            
      if(imu_odom_pose.empty())
	{
	  imu_odom_pose.insert(pair<double, pose_type>(imu -> header.stamp.toSec(), imu_pose));
	}
      else if(imu -> header.stamp.toSec() - imu_odom_pose.begin() -> first < 5)//10 s data for imu
	{
	  imu_odom_pose.insert(pair<double, pose_type>(imu -> header.stamp.toSec(),imu_pose));
	}
      else
	{
	  auto iter = imu_odom_pose.erase(std::begin(imu_odom_pose));//map首元素的key最小，所以删除它
	  //auto iter = odom_pose.begin();
	  while((float(imu -> header.stamp.toSec() - imu_odom_pose.begin() -> first)) > 5 && iter != imu_odom_pose.end())
	    {
	      iter = imu_odom_pose.erase(std::begin(imu_odom_pose));
	    }
	  imu_odom_pose.insert(pair<double, pose_type>(imu -> header.stamp.toSec(), imu_pose));
	}

      std::cout<<"imu_odom_pose:"<<imu_odom_pose.size()<<std::endl;      
      pose_lock.unlock();      
    }
}


void navigationnode::wheelCallback(const geometry_msgs::PoseStamped::ConstPtr& pose)
{
  if(!pose)
    return;

  std::unique_lock<std::shared_timed_mutex> pose_lock(mtx_wheel_odom_pose);

  pose_type wheel_pose;

  wheel_pose.x = pose->pose.position.x;
  wheel_pose.y = pose->pose.position.y;
  wheel_pose.theta = tf2::getYaw(pose->pose.orientation);

  if(wheel_odom_pose.empty())
    {
      wheel_odom_pose.insert(pair<double, pose_type>(pose -> header.stamp.toSec(),wheel_pose));
    }
  else if(pose -> header.stamp.toSec() - wheel_odom_pose.begin()->first < 5)//10 s data for imu
    {
      wheel_odom_pose.insert(pair<double, pose_type>(pose -> header.stamp.toSec(),wheel_pose));
    }
  else
    {
      auto iter = wheel_odom_pose.erase(std::begin(wheel_odom_pose));//map首元素的key最小，所以删除它
      //auto iter = odom_pose.begin();
      while((float(pose -> header.stamp.toSec() - wheel_odom_pose.begin() -> first)) > 5 && iter != wheel_odom_pose.end())
	{
	  iter = wheel_odom_pose.erase(std::begin(wheel_odom_pose));
	}
      wheel_odom_pose.insert(pair<double, pose_type>(pose -> header.stamp.toSec() , wheel_pose));
    }
  pose_lock.unlock();
  
}


// odom位姿队列定义为：std::map<common::Time,pf_vector_t> odom_pose;
bool navigationnode::getOdomPose(double& x, double& y, double& yaw,double time)
{
    //TODO:需要加锁，以确保在查询时刻t下odom队列位姿不会被改变
    std::unique_lock<std::shared_timed_mutex> pose_lock(mtx_wheel_odom_pose);

    if (wheel_odom_pose.size() == 0)
      {
        pose_lock.unlock();
        return false;//如果odom队列中没有数据，说明没有odom数据可用，返回false
      }
    const auto it = wheel_odom_pose.lower_bound(time);

    if (it == wheel_odom_pose.end())
      {
        x = prev(it) -> second.x;
        y = prev(it) -> second.y;
        yaw = prev(it) -> second.theta;
        pose_lock.unlock();
        return true;//如果需要查找的时间大于odom队列中的最晚时间，返回odom队列中最晚的位姿
      }

    if(it == wheel_odom_pose.begin())
      {
        x = it -> second.x;
        y = it -> second.y;
        yaw = it -> second.theta;
        pose_lock.unlock();
        return true;//如果需要查找的时间大于odom队列中的最晚时间，返回odom队列中最晚的位姿
      }
    
   const double duration = it -> first - prev(it) -> first;//Maybe map container will be exceed out!

    
   if (duration == 0)
    {
      pose_lock.unlock();
      return false;
    }
   

    const double factor = (time - prev(it) -> first) / duration;

    x = prev(it) -> second.x + (it -> second.x - prev(it) -> second.x) * factor;
    y = prev(it) -> second.y + (it -> second.y - prev(it) -> second.y) * factor;


    if(prev(it) -> second.theta * it-> second.theta >= 0)
      {
        yaw = prev(it) -> second.theta + (it -> second.theta - prev(it) -> second.theta) * factor;
      }
    else
      {
        if(fabs(prev(it) -> second.theta) < 0.1 || fabs(it -> second.theta) < 0.1)//欧拉角0度情况
	  {
            yaw = prev(it) -> second.theta + (it -> second.theta - prev(it) -> second.theta) * factor;
	  }
        else//欧拉角180度跳变情况
	  {
            double max_angle = it->second.theta;
            double min_angle ;
            if(max_angle > prev(it) -> second.theta)
	      {
                max_angle = prev(it) -> second.theta;
                min_angle = it -> second.theta;
	      } else
	      min_angle = prev(it) -> second.theta;
            double angle_dist = M_PI - max_angle + min_angle + M_PI;
            if(prev(it) -> second.theta < it -> second.theta)
	      angle_dist = -1 * angle_dist;

            yaw = prev(it) -> second.theta + angle_dist * factor;

            if(yaw > M_PI)
	      yaw = 2 * M_PI - yaw;
            else if(yaw < -M_PI)
	      yaw = 2 * M_PI + yaw;
	  }
      }
    //std::cout << "it_first:"<< it->first << " " << prev(it) -> first <<"duration:"<<duration<<"factor:" << factor<<"X:" <<x<<"Y:"<<y<<"yaw:"<<yaw<<std::endl;
    pose_lock.unlock();
    return true;
}


void navigationnode::laserScanToLDP(const point_cloud_type* std_point, LDP& laser, double max_range)
{
  
  laser = ld_alloc_new(std_point->array_size);

  laser->nrays = std_point->array_size;

  int num_rays = 0;
  //laser->nrays = msg->ranges.size();
  for (int i = 0; i < laser->nrays; i++) 
    {
      double r = std_point->r[i];
      if ((r > 0.15) && (r < max_range))
        {
	  laser->readings[i] = r;
	  laser->valid[i] = 1;
	  //laser->alpha[num_rays] = laser_incidence_angle->alpha[i];
        }else{
	laser->readings[i] = 0;
	laser->valid[i] = 0;
	//laser->alpha[num_rays] = GSL_NAN;
      }
      laser->theta[i] = std_point->alpha[i];
      laser->cluster[i] = -1;      
    }

  laser->max_theta = laser->theta[laser->nrays - 1];
  laser->min_theta = laser->theta[0];
  laser->odometry[0] = 0.0;
  laser->odometry[1] = 0.0;
  laser->odometry[2] = 0.0;

  laser->true_pose[0] = 0.0;
  laser->true_pose[1] = 0.0;
  laser->true_pose[2] = 0.0;
}


//accomplish pose calculation
void navigationnode::laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{      
  if(carto_map->map_->cells == NULL)
    {
      printf("Reading grid map is failed!\n");
      return;
    }

  unsigned sample_ratio = 0;
  unsigned bad_range_num_threshold = 0;
  
  point_cloud_raw_type point_cloud_raw(msg->ranges.size(), -PI, msg->angle_increment, CCW);
  point_cloud_raw_type point_cloud_map_raw(msg->ranges.size(), -PI, msg->angle_increment, CCW);

  LDP laser_source;
  LDP laser_target;


  if(msg->ranges.size() <= 720)
    {
      sample_ratio = 1;
      bad_range_num_threshold = 0;
    }
  else if(msg->ranges.size() > 720 && msg->ranges.size() <= 2500)
    {
      sample_ratio = 2;
      bad_range_num_threshold = 1;      
    }
  else if(msg->ranges.size() > 2500 && msg->ranges.size() <= 3600)
    {
      sample_ratio = 3;
      bad_range_num_threshold = 2;      
    }
  else if(msg->ranges.size() > 3600 && msg->ranges.size() <= 7200)
    {
      sample_ratio = 5;
      bad_range_num_threshold = 3;      
    }
  else if(msg->ranges.size() > 7200)
    {
      sample_ratio = 10;
      bad_range_num_threshold = 7;
    }

  double LaserRangeMax = msg->range_max;

  if(first_in_laser)
    {
      first_in_laser = false;
      int std_num_points = msg->ranges.size() / sample_ratio;
      point_cloud_prev.set_array_size(std_num_points);
      point_cloud_new.set_array_size(std_num_points);
      point_cloud_map.set_array_size(std_num_points);
      point_cloud_ref.set_array_size(std_num_points);
    }

  unsigned odo_OK_flag = 0;

  unsigned max_iter_num = 10;  // ICP迭代次数上限
  
  double Sigma_align_array[6] = { 0.0 };

  double corres_ratio = 0.0;

  
  //initialization
  if(init_pose_flag)
    {
            
      //memcpy(&point_cloud_raw.range[0],&msg->ranges[0],sizeof(point_cloud_raw.range));
      for(int i=0; i < msg->ranges.size(); i++)
	{
	  point_cloud_raw.range[i] = msg->ranges[i];
	}
	
      point_cloud_subsampling(&point_cloud_raw, &point_cloud_prev, sample_ratio, bad_range_num_threshold);
      basic_pretreatment(&point_cloud_prev, 0.26);  // 0.26的单位是rad，约等于15deg
	
      std::cout<<"Initial pose is:"<<laser_scanner_pose.x<<","<<laser_scanner_pose.y<<","<<laser_scanner_pose.theta<<std::endl;
      //extract scan from map based on laser_scanner_pose
      
      extract_scan_from_cartomap(carto_map, &point_cloud_map_raw, &laser_scanner_pose, LaserRangeMax);
      
  
      point_cloud_subsampling(&point_cloud_map_raw, &point_cloud_new, sample_ratio, bad_range_num_threshold);

      laserScanToLDP(&point_cloud_prev, laser_source, LaserRangeMax);
      laserScanToLDP(&point_cloud_new, laser_target, LaserRangeMax);

      params_s2m.first_guess[0] = 0.0;
      params_s2m.first_guess[1] = 0.0;      
      params_s2m.first_guess[2] = 0.0;
      params_s2m.laser_ref = laser_source;
      params_s2m.laser_sens = laser_target;
      
      csm_s2m->sm_icp(&params_s2m, &result_s2m, &corres_ratio);

      ld_free(laser_source);
      ld_free(laser_target);
      
      std::cout<<"correspondence ratio is:"<<corres_ratio<<std::endl;
      
      if(corres_ratio > 0.5)
	{
	  pose_type d_pose;
	  d_pose.x = result_s2m.x[0];
	  d_pose.y = result_s2m.x[1];
	  d_pose.theta = result_s2m.x[2];
	  
	  pose_type l_s_p = pose_transform(&d_pose, &laser_scanner_pose);
	  pose_x[cal_num] = l_s_p.x;
	  pose_y[cal_num] = l_s_p.y;
	  pose_th[cal_num] = l_s_p.theta;
	  laser_scanner_pose.x = l_s_p.x;
	  laser_scanner_pose.y = l_s_p.y;
	  laser_scanner_pose.theta = l_s_p.theta;
	  
	  cal_num++;

	  if(cal_num == NumcalculationInitPose)
	    {
	     //TODO:publish laser_scanner_pose ->amcl(initilization pose)	  	  
             init_pose_flag = false;
             pose_abnormal_count = 1;
	               
	     //laser_scanner_pose = pose_transform(&odo,&laser_scanner_pose);
             double x = 0.0,y = 0.0,th = 0.0;
	     for(int i = 0; i < NumcalculationInitPose; i++)
	       {
		 x += pose_x[i];
		 y += pose_y[i];
               }
	     laser_scanner_pose.x = x / NumcalculationInitPose;
	     laser_scanner_pose.y = y / NumcalculationInitPose;
	     laser_scanner_pose.theta = pose_th[0];

	     
	     P_element[0] = *(result_s2m.cov_x_m->data);
	     P_element[1] = *(result_s2m.cov_x_m->data + 1);
	     P_element[2] = *(result_s2m.cov_x_m->data + 4);
	     P_element[3] = *(result_s2m.cov_x_m->data + 6);
	     P_element[4] = *(result_s2m.cov_x_m->data + 7);
	     P_element[5] = *(result_s2m.cov_x_m->data + 8);

	     point_cloud_ref = point_cloud_prev;
	     ref_map_pose = laser_scanner_pose;
	     memcpy(refmap_sigma_array, P_element, sizeof(P_element));

	     function_pub(laser_scanner_pose, P_element, msg, laserpose_pub_);//laser pose to move_base
	     printf("Pose initialization success!\n");

	     printf("*********************************** AGV Pose Initialization Summary ***********************************\n");
	     printf("laser pose: (x,y) = (%8.4f, %8.4f)m, theta = %8.4f deg\n", laser_scanner_pose.x, laser_scanner_pose.y, laser_scanner_pose.theta*180/PI);
	     printf("pose std: std_x = %9.7fm, std_y = %9.7fm, std_theta = %9.7fdeg\n", sqrt(P_element[0]), sqrt(P_element[2]), sqrt(P_element[5]) * 180 / PI);
	     printf("***********************************************************************************************\n");

	     
            }
	}
      else
	{
	  printf("Try to initialize!\n");
	  init_pose_flag = true;
	}
      last_scan_time = msg -> header.stamp;
    }
  else
    {      
      for(int i = 0; i < msg->ranges.size(); i++)
	{
	  point_cloud_raw.range[i] = msg->ranges[i];	  
	}
      point_cloud_subsampling(&point_cloud_raw, &point_cloud_new, sample_ratio, bad_range_num_threshold);
      basic_pretreatment(&point_cloud_new, 0.26);  // 0.26的单位是rad，约等于15deg
      
      //odom calculation
      odo = odo_prev;
      
      laserScanToLDP(&point_cloud_prev, laser_source, LaserRangeMax);
      laserScanToLDP(&point_cloud_new, laser_target, LaserRangeMax);

      params_s2s.first_guess[0] = odo.x;
      params_s2s.first_guess[1] = odo.y;      
      params_s2s.first_guess[2] = odo.theta;
      params_s2s.laser_ref = laser_source;
      params_s2s.laser_sens = laser_target;
           
      csm_s2s->sm_icp(&params_s2s, &result_s2s, &corres_ratio);
            
      odo.x = result_s2s.x[0];
      odo.y = result_s2s.x[1];
      odo.theta = result_s2s.x[2];
      
      ld_free(laser_source);
      ld_free(laser_target);
      
      //when laser-odom is not good,use imudata/wheelodomdata
       if(corres_ratio < 0.05)
	{
	  
	  pose_type last_wheel_pose, curt_wheel_pose;
	  bool last_get_flag = getOdomPose(last_wheel_pose.x, last_wheel_pose.y, last_wheel_pose.theta, last_scan_time.toSec());
	  bool curt_get_flag = getOdomPose(curt_wheel_pose.x, curt_wheel_pose.y, curt_wheel_pose.theta, msg -> header.stamp.toSec());
	  
	  if(last_get_flag && curt_get_flag)//both get pose successfully!
	    {	      
	      pose_type deta_odo = pose_delta_transform(&curt_wheel_pose, &last_wheel_pose);//delta-laser-pose from t to t-1(In wheel-odom coordinate)  
	      odo.x = deta_odo.x;
	      odo.y = deta_odo.y;
	      odo.theta = deta_odo.theta;	      	    
	    }

	  //if imu_wheel_pose is valid,we better believe the imu's yaw angle!
	  std::unique_lock<std::shared_timed_mutex> pose_lock(mtx_imu_odom_pose);
	  if(!imu_odom_pose.empty())
	    {
	      double cur_theta,prev_theta;
	      cur_theta = imu_odom_pose.lower_bound(msg -> header.stamp.toSec())->second.theta;
	      prev_theta = imu_odom_pose.lower_bound(last_scan_time.toSec())->second.theta;

	      if(imu_odom_pose.rbegin()->first < msg -> header.stamp.toSec())
	         {
	         cur_theta = imu_odom_pose.rbegin()->second.theta;
	         //std::cout<<"can not find imu_odom_theta"<<std::endl;
	         }
	      if(imu_odom_pose.rbegin()->first < last_scan_time.toSec())
	         {
		   cur_theta = 0.0;
		   prev_theta = 0.0;
	         }
	      odo.theta = cur_theta - prev_theta;
	      odo.theta = atan2(sin(odo.theta),cos(odo.theta));
	    }	  
	  pose_lock.unlock();
	}

       //std::cout<<"*************odo**********************:"<<odo.x<<","<<odo.y<<","<<odo.theta<<std::endl;

      odo_prev = odo;
       
      point_cloud_prev = point_cloud_new;//don't forget

      point_cloud_motion_compensation(&point_cloud_new, &odo);


      //scan to submap matching			  
      delta_pose = pose_transform( &odo, &delta_pose_prev );

      laserScanToLDP(&point_cloud_ref, laser_source, LaserRangeMax);
      laserScanToLDP(&point_cloud_new, laser_target, LaserRangeMax);

      params_s2k.first_guess[0] = delta_pose.x;
      params_s2k.first_guess[1] = delta_pose.y;      
      params_s2k.first_guess[2] = delta_pose.theta;
      params_s2k.laser_ref = laser_source;
      params_s2k.laser_sens = laser_target;
      
      csm_s2k->sm_icp(&params_s2k, &result_s2k, &corres_ratio);

      ld_free(laser_source);
      ld_free(laser_target);
      
      delta_pose.x = result_s2k.x[0];
      delta_pose.y = result_s2k.x[1];      
      delta_pose.theta = result_s2k.x[2];
      
      Sigma_align_array[0] = *(result_s2k.cov_x_m->data);
      Sigma_align_array[1] = *(result_s2k.cov_x_m->data + 1);
      Sigma_align_array[2] = *(result_s2k.cov_x_m->data + 4);
      Sigma_align_array[3] = *(result_s2k.cov_x_m->data + 6);
      Sigma_align_array[4] = *(result_s2k.cov_x_m->data + 7);
      Sigma_align_array[5] = *(result_s2k.cov_x_m->data + 8); 
      
      laser_scanner_pose = pose_transform( &delta_pose, &ref_map_pose );			  
      calcu_cov(refmap_sigma_array, Sigma_align_array, P_element, &ref_map_pose, &delta_pose);
      P_element[0] += 7e-6; P_element[2] += 7e-6; P_element[5] += 1e-7;

      //std::cout<<"*************delta_pose**********************:"<<delta_pose.x<<","<<delta_pose.y<<","<<delta_pose.theta<<std::endl;
      
      
      if(corres_ratio < 0.55)
	{				
	  point_cloud_ref = point_cloud_new;
			    
	  delta_pose_prev = { 0.0, 0.0, 0.0 }; delta_pose = { 0.0, 0.0, 0.0 };				    				   				    
	  //extract scan from map, to be ekf's obs-para
	  extract_scan_from_cartomap(carto_map, &point_cloud_map_raw, &laser_scanner_pose, LaserRangeMax);

	 
	  point_cloud_subsampling(&point_cloud_map_raw, &point_cloud_new, sample_ratio, bad_range_num_threshold);
	  basic_pretreatment(&point_cloud_new, 0.26);	  


	  laserScanToLDP(&point_cloud_ref, laser_source, LaserRangeMax);
	  laserScanToLDP(&point_cloud_new, laser_target, LaserRangeMax);

	  params_k2m.first_guess[0] = 0.0;
	  params_k2m.first_guess[1] = 0.0;      
	  params_k2m.first_guess[2] = 0.0;
	  params_k2m.laser_ref = laser_source;
	  params_k2m.laser_sens = laser_target;
      
	  csm_k2m->sm_icp(&params_k2m, &result_k2m, &corres_ratio);

	  //std::cout<<"submap to map match correspondence ratio is:"<<corres_ratio<<std::endl;

	  ld_free(laser_source);
	  ld_free(laser_target);
      
	  delta_pose.x = result_k2m.x[0];
	  delta_pose.y = result_k2m.x[1];      
	  delta_pose.theta = result_k2m.x[2];
      
	  Sigma_align_array[0] = *(result_k2m.cov_x_m->data);
	  Sigma_align_array[1] = *(result_k2m.cov_x_m->data + 1);
	  Sigma_align_array[2] = *(result_k2m.cov_x_m->data + 4);
	  Sigma_align_array[3] = *(result_k2m.cov_x_m->data + 6);
	  Sigma_align_array[4] = *(result_k2m.cov_x_m->data + 7);
	  Sigma_align_array[5] = *(result_k2m.cov_x_m->data + 8);

      
	  ref_map_pose = pose_transform(&delta_pose, &laser_scanner_pose);
	  
	  memcpy(refmap_sigma_array, Sigma_align_array, sizeof(Sigma_align_array));
	  refmap_sigma_array[0] += 9e-5; refmap_sigma_array[2] += 9e-5; refmap_sigma_array[5] += 1e-6;//(7e-6)	  
	  robust_ekf_pose(&laser_scanner_pose, P_element, ref_map_pose, refmap_sigma_array, 50);		  
	  odo = { 0.0, 0.0, 0.0 };
	  delta_pose_prev = { 0.0, 0.0, 0.0 }; delta_pose = { 0.0, 0.0, 0.0 };
	  ref_map_pose = laser_scanner_pose;
	  memcpy(refmap_sigma_array, P_element, sizeof(P_element));

	  if (corres_ratio < 0.15)
	    {
	      if (++pose_abnormal_count > 4)
		{
		  printf("Continuously 3 times scan to map's matching overlap ratio is less than 0.15!\n");
		  init_pose_flag = true;//Try to initialize!!!
                  cal_num = 0;
		  return;
		}
	    }
	  else
	    {
	      pose_abnormal_count = 1;
	    }
	}
      delta_pose_prev = delta_pose;            

      
      printf("*********************************** AGV Pose Update Summary ***********************************\n");
      printf("laser pose: (x,y) = (%8.4f, %8.4f)m, theta = %8.4f deg\n", laser_scanner_pose.x, laser_scanner_pose.y, laser_scanner_pose.theta*180/PI);
      printf("pose std: std_x = %9.7fm, std_y = %9.7fm, std_theta = %9.7fdeg\n", sqrt(P_element[0]), sqrt(P_element[2]), sqrt(P_element[5]) * 180 / PI);
      printf("***********************************************************************************************\n");
      
             
      function_pub(laser_scanner_pose, P_element, msg, laserpose_pub_);

      last_scan_time = msg -> header.stamp;
    }
    
}

//accomplish pose publication
void navigationnode::function_pub(pose_type pose,double sigma_array[6],const sensor_msgs::LaserScan::ConstPtr& msg,ros::Publisher pose_pub)
 {
   geometry_msgs::PoseWithCovarianceStamped p;
      // Fill in the header
   p.header.frame_id = "map";
   p.header.stamp = msg -> header.stamp;
   //p.header.stamp = ros::Time::now();
      // Copy in the pose
   p.pose.pose.position.x = pose.x;
   p.pose.pose.position.y = pose.y;

   tf2::Quaternion q;
   q.setRPY(0, 0, pose.theta);
   tf2::convert(q, p.pose.pose.orientation);
      // Copy in the covariance, converting from 3-D to 6-D


   p.pose.covariance[0] = sigma_array[0];
   p.pose.covariance[1] = sigma_array[1];
   p.pose.covariance[5] = sigma_array[3];
   p.pose.covariance[6] = sigma_array[1];
   p.pose.covariance[7] = sigma_array[2];
   p.pose.covariance[11] = sigma_array[4];
   p.pose.covariance[30] = sigma_array[3];
   p.pose.covariance[31] = sigma_array[4];
   p.pose.covariance[35] = sigma_array[5];
   
   pose_pub.publish(p);//publish base_link's pose (in the fixed_frame) to Amcl Node
 }


int main(int argc, char** argv)
{  
  ros::init(argc, argv, "relocalizationCensiICP");

  ros::NodeHandle nh;
  
  navigationnode navigation;
    
  ros::Rate r(100.0);
  
  while(nh.ok())
  {
    ros::spinOnce();
    
    r.sleep();
  }
  
  return(0);
}
